// Meena Abdou
// CPEG 467 - ERA MOBI Robot project
// 10/21/2010

/*
 * Walks down a hallway until front <= 1.5 or right <= 0.5 or left <= 0.5.
 * Only turns left for now.  This program as is was able to walk around Evans 
 * Hall all on its own.
 * 
 * This program tests the built in odometry functions.
 */

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <fstream>
//#include <ctime>
//#include <vector>

#include "args.h"

#define FRONT 327
#define RIGHT 110
#define LEFT 546

PlayerCc::PlayerClient robot(gHostname, gPort);
PlayerCc::Position2dProxy pp(&robot, gIndex);
PlayerCc::LaserProxy lp(&robot, gIndex);
//std::vector<double> XCoords, YCoords, YawCoords, Time;
//std::time_t currentTime;
std::ofstream fout;
bool turning = false;

//Find the minimum value of a range of sensors
double checkRange(double min, double max){
  double minR = 5.6;
  for(int i = min; i < max; i++){
    if(lp[i] < minR)
      minR=lp[i];
  }
  return minR;
}

void turn(double speed, double turnrate){
  pp.SetSpeed(speed, turnrate);
}

void goStraight(){
  pp.SetSpeed(0.5, 0.0);
}

void odometry(){
  robot.Read();

  std::time_t timeNow = time(NULL);
  double x = pp.GetXPos();
  double y = pp.GetYPos();
  double yaw = pp.GetYaw();

  //XCoords.push_back(x);
  //YCoords.push_back(y);
  //YawCoords.push_back(yaw);
  //Time.push_back(timeNow-currentTime);

  //fout << "X: "; 
  fout.width(13); 
  fout << x; 
  fout << " ";
  //fout << "Y: ";
  fout.width(13);
  fout << y;
  fout << " ";
  //fout << "Yaw: ";
  fout.width(13);
  fout << PlayerCc::rtod(yaw);
  fout << " ";
  //fout << "Right: "; 
  fout << lp[RIGHT];
  fout << " ";
  //fout << "Left: ";
  fout << lp[LEFT] << "\n";

  //std::cout << "Current Position\n";
  //std::cout << "X: " << pp.GetXPos() << " Y: " << pp.GetYPos() << " Yaw: " << pp.GetYaw() << "\n";
  
}

void drive(){
  robot.Read();
  double front, right, left;
  /* Using only the certain sensors - front, left, right */
  front = lp[327];
  right = /*(lp[109] + lp[110])/2;*/ lp[218];
  left = /*(lp[545] + lp[546])/2;*/ lp[437];
  
  if(){
    //turn(0.0, 0.375); // turn left
    std::cout << "At intersection\n";
    turn(0.0, 0.375);
    //exit(0);
  }


  else if(right <= 0.5){
    //newspeed = 0.0;
    //newturnrate = 0.375;
    //fout << "\nNow Turning Left with turn rate 0.375\n";
    turn(0.0, 0.375); // turn left
  }
  else if(left <= 0.5){
    //newspeed = 0.0;
    //newturnrate = -0.375;
    //fout << "\nNow Turning Right with turn rate 0.375\n";
    turn(0.0, -0.375); // turn right
  }
  else if(right <= 1.0){
    //newspeed = 0.2;
    //newturnrate = 0.375;
    //fout << "\nNow Turning Left with turn rate 0.375\n";
    turn(0.2, 0.375); // turn left
  }
  else if(left <= 1.0){
    //newspeed = 0.2;
    //newturnrate = -0.375;
    //fout << "\nNow Turning Right with turn rate 0.375\n";
    turn(0.2, -0.375); // turn right
  }
  else if(front <= 1.5){
    //newspeed = 0.2;
    //newturnrate = 0.375;
    //fout << "\nNow Turning Left with turn rate 0.375\n";
    turn(0.2, 0.375); // turn left
  }
  else{
    //newspeed = 0.5;
    //newturnrate = 0.0;
    goStraight();
  }
      
  //printf("Front: %f\nRight: %f\nLeft: %f\n\n", front, right, left);      
}

int main(int argc, char **argv)
{
  parse_args(argc,argv);
  fout.open("OutputData.txt");

  if(!fout.is_open()){
    std::cerr << "Error opening file\n";
    exit(-1);
  }

  fout.setf(std::ios::fixed, std::ios::floatfield);
  fout.precision(8);

  //currentTime = time(NULL);

  // we throw exceptions on creation if we fail
  try
  {
    
    std::cout << robot << std::endl;

    pp.SetMotorEnable (true);
    pp.ResetOdometry();
    
    //double newspeed = 0;
    //double newturnrate = 0;
    //double front, right, left;
    
    robot.Read(); //here so GetRange doesn't segfault

    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();
      drive();
      odometry();
    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
